#include <libfreenect.hpp>
#include <iostream>
#include <vector>
#include <cmath>
#include <math.h>
#include <pthread.h>
#include <opencv/cv.h>
#include <opencv2/opencv.hpp>
#include <opencv/cxcore.h>
#include <opencv/highgui.h>


using namespace cv;
using namespace std;

//Definizione della classe Mutex
class Mutex {
public:
	Mutex() {
		pthread_mutex_init( &m_mutex, NULL );
	}
	void lock() {
		pthread_mutex_lock( &m_mutex );
	}
	void unlock() {
		pthread_mutex_unlock( &m_mutex );
	}
private:
	pthread_mutex_t m_mutex;
};

class MyFreenectDevice : public Freenect::FreenectDevice {
public:
	MyFreenectDevice(freenect_context *_ctx, int _index):Freenect::FreenectDevice(_ctx, _index),
	m_buffer_depth	(FREENECT_DEPTH_11BIT),
	m_buffer_rgb(FREENECT_VIDEO_RGB),
	m_gamma(2048), 
	m_new_rgb_frame(false), 
	m_new_depth_frame(false),
	depthMat(Size(640,480),CV_16UC1), 
	rgbMat(Size(640,480),CV_8UC3,Scalar(0)), 
	ownMat(Size(640,480),CV_8UC3,Scalar(0))
	{
		for( unsigned int i = 0 ; i < 2048 ; i++) {
			float v = i/2048.0;
			v = std::pow(v, 3)*6;
			m_gamma[i] = v*6*256;
		}
	}


	void VideoCallback(void* _rgb, uint32_t timestamp) {
		m_rgb_mutex.lock();
		uint8_t* rgb = static_cast<uint8_t*>(_rgb);
		rgbMat.data = rgb;
		m_new_rgb_frame = true;
		m_rgb_mutex.unlock();
	};

	void DepthCallback(void* _depth, uint32_t timestamp) {
		m_depth_mutex.lock();
		uint16_t* depth = static_cast<uint16_t*>(_depth);
		depthMat.data = (uchar*) depth;
		m_new_depth_frame = true;
		m_depth_mutex.unlock();
	}

	bool getVideo(Mat& output) {
		m_rgb_mutex.lock();
		if(m_new_rgb_frame) {
			cv::cvtColor(rgbMat, output, CV_RGB2BGR);
			m_new_rgb_frame = false;
			m_rgb_mutex.unlock();
			return true;
		} else {
			m_rgb_mutex.unlock();
			return false;
		}
	}

	bool getDepth(Mat& output) {
		m_depth_mutex.lock();
		if(m_new_depth_frame) {
			depthMat.copyTo(output);
			m_new_depth_frame = false;
			m_depth_mutex.unlock();
			return true;
		} else {
			m_depth_mutex.unlock();
			return false;
		}
	}

private:
	std::vector<uint8_t> m_buffer_depth;
	std::vector<uint8_t> m_buffer_rgb;
	std::vector<uint16_t> m_gamma;
	Mat depthMat;
	Mat rgbMat;
	Mat ownMat;
	Mutex m_rgb_mutex;
	Mutex m_depth_mutex;
	bool m_new_rgb_frame;
	bool m_new_depth_frame;
};

int main(int argc, char **argv) {


	bool end(false);
	char k;
	int ii;

	//Definizioni immagini
	Mat rgbMat(Size(640,480),CV_8UC3,Scalar(0));
	Mat depthMat(Size(640,480),CV_16UC1);
	Mat hsvMat(Size(640,480),CV_8UC3,Scalar(0));


	//Dichiarazione variabili palla
	Scalar infPalla=Scalar(0,183,152);
	Scalar supPalla=Scalar(9,253,255);

	RotatedRect maxRettangoloPalla,box;
	bool pallaRiconosciuta,inizioRiconoscimento;
	Mat img_palla_filt(Size(640,480),CV_8UC1,Scalar(0));
	Mat img_palla_filt2(Size(640,480),CV_8UC1,Scalar(0));
	vector<vector<Point> > contours_palla,contours_porta,contours_ostacolo,result;
	Mat pointsfPalla;


	//Dichiarazione variabili porta
	Scalar infPorta=Scalar(61,139,135);
	Scalar supPorta=Scalar(118,255,255);

	bool portaRiconosciuta=false;
	Mat img_porta_filt(Size(640,480),CV_8UC1,Scalar(0));
	Mat img_porta_filt2(Size(640,480),CV_8UC1,Scalar(0));
	Mat pointsfPorta;

	//Dichiarazione variabili ostacolo
	Scalar infOstacolo=Scalar(118,255,255);
	Scalar supOstacolo=Scalar(118,255,255);

    bool ostacoloRiconosciuto=false;
	Rect maxRettangoloOstacolo;
	Mat img_ostacolo_filt(Size(640,480),CV_8UC1,Scalar(0));
	Mat img_ostacolo_filt2(Size(640,480),CV_8UC1,Scalar(0));
	Mat pointsfOstacolo;




	Freenect::Freenect freenect;
	MyFreenectDevice& device = freenect.createDevice<MyFreenectDevice>(0);
	//device.setDepthFormat(FREENECT_DEPTH_MM);
	device.startVideo();
	device.startDepth();





	while (!end) {


			device.getVideo(rgbMat);
			device.getDepth(depthMat);

			//filtraggio delle immagini
			cvtColor(rgbMat,hsvMat,CV_BGR2HSV);
			inRange(hsvMat,infPalla, supPalla,img_palla_filt);
			dilate(img_palla_filt,img_palla_filt, Mat(), Point(-1,-1), 1, NULL,NULL );
			erode(img_palla_filt,img_palla_filt, Mat(), Point(-1,-1), 1, NULL,NULL );
			bilateralFilter(img_palla_filt,img_palla_filt2,7,150,150);

			inRange(hsvMat,infPorta, supPorta,img_porta_filt);
			dilate(img_porta_filt,img_porta_filt, Mat(), Point(-1,-1), 1, NULL,NULL );
			erode(img_porta_filt,img_porta_filt, Mat(), Point(-1,-1), 1, NULL,NULL );
			bilateralFilter(img_porta_filt,img_porta_filt2,7,150,150);

			inRange(hsvMat,infOstacolo, supOstacolo,img_ostacolo_filt);
			dilate(img_ostacolo_filt,img_ostacolo_filt, Mat(), Point(-1,-1), 1, NULL,NULL );
			erode(img_ostacolo_filt,img_ostacolo_filt, Mat(), Point(-1,-1), 1, NULL,NULL );
			bilateralFilter(img_ostacolo_filt,img_ostacolo_filt2,7,150,150);



			//Riconoscimento della palla

	        findContours(img_palla_filt2, contours_palla, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

			pallaRiconosciuta=false;
			inizioRiconoscimento=true;

			for(size_t i = 0; i < contours_palla.size(); i++)
			{
				size_t count = contours_palla[i].size();

				if( count < 6 )
					continue;

				Mat(contours_palla[i]).convertTo(pointsfPalla, CV_32F);
				box = fitEllipse(pointsfPalla);
				//box.angle = -box.angle;

				if( MAX(box.size.width, box.size.height) > MIN(box.size.width, box.size.height)*30 )
					continue;
				if(box.size.height*box.size.width<1000)
					continue;
				if(inizioRiconoscimento){
					inizioRiconoscimento=false;
					maxRettangoloPalla=box;
					pallaRiconosciuta=true;
				}else if(box.size.height*box.size.width>maxRettangoloPalla.size.height*maxRettangoloPalla.size.width)
					maxRettangoloPalla=box;
			}

			if(pallaRiconosciuta){

				circle( rgbMat, maxRettangoloPalla.center, MIN(maxRettangoloPalla.size.width/2,maxRettangoloPalla.size.height/2), Scalar(255,255,255), 1, 8, 0 );
				circle( rgbMat, maxRettangoloPalla.center, 3, Scalar(255,255,255), -1, 8, 0 );
			}




			//Riconoscimento della porta

				findContours(img_porta_filt2, contours_porta, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

				vector<vector<Point2f> > contours_poligono_porta( contours_porta.size() );
				vector<Rect> rettangoli_porta( contours_porta.size() );
				Rect maxRettangoloPorta;


					for(ii=0;ii<contours_porta.size();ii++){
						Mat(contours_porta[ii]).convertTo(pointsfPorta, CV_32F);
						approxPolyDP( pointsfPorta, contours_poligono_porta[ii], 3, true );
						rettangoli_porta[ii] = boundingRect( Mat(contours_poligono_porta[ii]) );
					}


					for(ii=1;ii<contours_porta.size();ii++){
						if(rettangoli_porta[ii].area()>maxRettangoloPorta.area()){
							portaRiconosciuta=true;
							maxRettangoloPorta=rettangoli_porta[ii];
						}
					}

					rectangle( rgbMat,maxRettangoloPorta,Scalar(255,255,255));
					Point centroRettangoloPorta=Point(maxRettangoloPorta.x+maxRettangoloPorta.width/2,maxRettangoloPorta.y+maxRettangoloPorta.height/2);
					circle( rgbMat, centroRettangoloPorta, 3, Scalar(255,255,255), -1, 8, 0 );



					//Riconoscimento dell'ostacolo

/*

					findContours(img_ostacolo_filt2, contours_ostacolo, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
					vector<vector<Point2f> > contours_poligono_ostacolo( contours_ostacolo.size() );
					vector<Rect> rettangoli_ostacolo( contours_ostacolo.size());
					Rect maxRettangoloOstacolo;


					for(ii=0;ii<contours_ostacolo.size();ii++){
						Mat(contours_ostacolo[ii]).convertTo(pointsfOstacolo, CV_32F);
						approxPolyDP( pointsfOstacolo, contours_poligono_ostacolo[ii], 3, true );
						rettangoli_ostacolo[ii] = boundingRect( Mat(contours_poligono_ostacolo[ii]) );
					}


					for(ii=1;ii<contours_ostacolo.size();ii++){
						if(rettangoli_ostacolo[ii].area()>maxRettangoloOstacolo.area()){
							ostacoloRiconosciuto=true;
							maxRettangoloOstacolo=rettangoli_ostacolo[ii];
						}
					}

					rectangle( rgbMat,maxRettangoloOstacolo,Scalar(255,255,255));
					Point centroRettangoloOstacolo=Point(maxRettangoloOstacolo.x+maxRettangoloOstacolo.width/2,maxRettangoloOstacolo.y+maxRettangoloOstacolo.height/2);
					circle( rgbMat, centroRettangoloOstacolo, 3, Scalar(255,255,255), -1, 8, 0 );*/


			//int k,t,prof_palla,prof_porta;
			//double distanza_palla_porta=100,distanza_des=100,prof_des;
			//vector<Point2f> center( contours_porta.size() );
			//vector<float> raggio( contours_porta.size() );
			//double beta;




			imshow("IMG_RGB",rgbMat);
            k = cvWaitKey(5);
			if( k == 27){
				destroyAllWindows();
				break;
			}
	}


	device.stopVideo();
	device.stopDepth();
	return 0;
}
